Line data    Source code

       1              : #include "RVCController.hpp"
       2              : #include "CleaningController.hpp"
       3              : #include "NavigationController.hpp"
       4              : 
       5           64 : RVCController::RVCController(NavigationController& nav, CleaningController& cleaning)
       6           64 :     : nav_(nav), cleaning_(cleaning), state_(State::IDLE), leftClear_(false) {}
       7              : 
       8           32 : void RVCController::start() {
       9           32 :     if (state_ == State::CLEANING ||
      10           31 :         state_ == State::REVERSING ||
      11           32 :         state_ == State::REVERSING_CHECKING_RIGHT) return;
      12           31 :     cleaning_.startCleaning();
      13           31 :     nav_.moveForward();
      14           31 :     state_ = State::CLEANING;
      15           32 : }
      16              : 
      17           23 : void RVCController::frontObstacleDetected() {
      18           23 :     if (state_ == State::REVERSING ||
      19           23 :         state_ == State::REVERSING_CHECKING_RIGHT) return;
      20           23 :     cleaning_.stopCleaning();
      21           23 :     nav_.stop();
      22           23 :     state_ = State::STOPPED;
      23           23 : }
      24              : 
      25           55 : void RVCController::sideStatus(Direction /*direction*/, bool clear) {
      26           55 :     switch (state_) {
      27              :         case State::STOPPED:
      28              :             // SD-002: sideStatus(LEFT) 수신 — rotateRight로 우측 스캔 준비
      29           20 :             leftClear_ = clear;
      30           20 :             nav_.rotateRight();
      31           20 :             state_ = State::STOPPED_CHECKING_RIGHT;
      32           20 :             break;
      33              : 
      34              :         case State::STOPPED_CHECKING_RIGHT:
      35              :             // SD-002: sideStatus(RIGHT) 수신 — rotateLeft로 정면 복귀 후 방향 결정
      36           19 :             nav_.rotateLeft();
      37           19 :             if (leftClear_) {
      38            4 :                 nav_.turn(Direction::LEFT);
      39            4 :                 cleaning_.startCleaning();
      40            4 :                 nav_.moveForward();
      41            4 :                 state_ = State::CLEANING;
      42           19 :             } else if (clear) {
      43            3 :                 nav_.turn(Direction::RIGHT);
      44            3 :                 cleaning_.startCleaning();
      45            3 :                 nav_.moveForward();
      46            3 :                 state_ = State::CLEANING;
      47            3 :             } else {
      48              :                 // E1: 좌·우 모두 막힘 → UC-003
      49           12 :                 nav_.moveBackward();
      50           12 :                 state_ = State::REVERSING;
      51              :             }
      52           19 :             break;
      53              : 
      54              :         case State::REVERSING:
      55              :             // SD-003 루프: sideStatus(LEFT) 수신
      56           10 :             if (clear) {
      57            4 :                 nav_.stop();
      58            4 :                 nav_.turn(Direction::LEFT);
      59            4 :                 nav_.moveForward();
      60            4 :                 cleaning_.startCleaning();
      61            4 :                 state_ = State::CLEANING;
      62            4 :             } else {
      63            6 :                 nav_.rotateRight();
      64            6 :                 state_ = State::REVERSING_CHECKING_RIGHT;
      65              :             }
      66           10 :             break;
      67              : 
      68              :         case State::REVERSING_CHECKING_RIGHT:
      69              :             // SD-003 루프: sideStatus(RIGHT) 수신 — rotateLeft로 정면 복귀
      70            5 :             nav_.rotateLeft();
      71            5 :             if (clear) {
      72            3 :                 nav_.stop();
      73            3 :                 nav_.turn(Direction::RIGHT);
      74            3 :                 nav_.moveForward();
      75            3 :                 cleaning_.startCleaning();
      76            3 :                 state_ = State::CLEANING;
      77            3 :             } else {
      78            2 :                 state_ = State::REVERSING;
      79              :             }
      80            5 :             break;
      81              : 
      82              :         default:
      83            1 :             break;
      84              :     }
      85           55 : }
      86              : 
      87            1 : void RVCController::dustDetected() {
      88            1 :     cleaning_.boostCleaning();
      89            1 : }
      90              : 
      91            2 : void RVCController::dustDetected(float dustLevel) {
      92            2 :     if (dustLevel < 0.0f) return;
      93            1 :     cleaning_.boostCleaning();
      94            2 : }
      95              : 
      96            6 : void RVCController::changeState(int newState) {
      97            6 :     if (newState < 0 || newState > 1) return;  // 유효 범위: 0=IDLE, 1=CLEANING
      98            4 :     State target = (newState == 0) ? State::IDLE : State::CLEANING;
      99            4 :     if (state_ == target) return;              // 동일 상태 → 전이 없음
     100            3 :     state_ = target;
     101            6 : }